package com.googlecode.curvedmetromaps.core.logic.costfunction;

import org.jbox2d.common.Vec2;

import com.googlecode.curvedmetromaps.core.geometry.BezierUtil;
import com.googlecode.curvedmetromaps.core.model.MetroEdge;
import com.googlecode.curvedmetromaps.core.model.MetroMap;

public class BezierCurvatureCost extends BaseCost {
	private static final int	sampleSize	= 100;

	public BezierCurvatureCost(MetroMap map) {
		super(map);
		createWeight("base", "Base weight", 0.02);
	}

	@Override
	public String getName() {
		return "Bezier curvature";
	}

	@Override
	public double calculateCost() {
		double result = 0;
		for (MetroEdge edge : map.edgeArray) {
			double cost = edgeCurvatureCost(edge.toArray());
			result += cost;
		}
		// Calculate average cost per edge
		result /= map.edgeArray.length;
		return getWeight("base") * result;
	}

	private double edgeCurvatureCost(Vec2[] v) {
		double result = 0;
		double lastCurvature = 0;
		for (int i = 1; i < sampleSize; i++) {
			Vec2[] left = new Vec2[3];
			BezierUtil.subdivide(v, (float) i / (float) sampleSize, left, null);
			double curvature = BezierUtil.curvature(left);
			result += Math.abs(curvature - lastCurvature);
			lastCurvature = curvature;
		}
		return result;
	}
}
